package com.arctic.coding.marvin;

import java.io.BufferedReader;
import java.io.InputStreamReader;
import java.util.StringTokenizer;

import com.arctic.coding.marvin.car.CameraControl;
import com.arctic.coding.marvin.car.EngineControl;
import com.arctic.coding.marvin.car.WheelControl;
import com.arctic.coding.marvin.sensor.BatteryHandler;
import com.arctic.coding.marvin.sensor.UltraSonicHandler;

import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.RConsole;
import cpm.arctic.coding.marvin.communication.BluetoothHandler;
import cpm.arctic.coding.marvin.communication.CommunicationException;

public class ControlServer extends Thread {

	public EngineControl 		engine;
	public WheelControl			wheel;
	public CameraControl		camera;
	public UltraSonicHandler 	usSensor;
	
	public ControlServer(EngineControl engine, WheelControl wheel, CameraControl camera) {
		this.engine = engine;
		this.wheel	= wheel;
		this.camera	= camera;
	}
	
	/**
	 * Liest die Kommandos ueber die Bluetoothverbindung ein und verarbeitet sie
	 */
	@Override
	public void run() {		
			
			try {
				
				BluetoothHandler.init();
				
				engine.startEngine();
								
				usSensor = new UltraSonicHandler(new UltrasonicSensor(SensorPort.S1), engine);
				//usSensor.start(); Deaktiviert, da keine korrekten Werte vom Sensor kommen
								
				if (BluetoothHandler.getInstance().connectToPhone()) {
					
					new BatteryHandler().start();
					
					String line;				
					BufferedReader buf = new BufferedReader(new InputStreamReader(BluetoothHandler.getInstance().getInputStream()));
					
					while ((line = buf.readLine()) != null) {
						LCD.clear(0);
						LCD.drawString(line, 0, 0, true);
						this.execCommand(line);					
						line = new String();
						System.gc();
					}						
					
				}			
				
			} catch (Exception ex) { 
				LCD.drawString(ex.getMessage(), 0, 3, true);
				LCD.drawString(""+ ex, 0, 4, true);		
			}
			
			engine.stopEngine();
			
	}
	
	/**
	 * Prueft das Kommando. Bei Positiver Pruefung wird das Kommando geparst und ausgefuehrt
	 * 
	 * @param command Das auszufuehrende Kommando
	 */
	private void execCommand(String command) {
	
		StringTokenizer tokenizer = new StringTokenizer(command, ";");
		
		if (tokenizer.countTokens() == 3) {
			
			String nxtName 		= tokenizer.nextToken();
			String nxtCommand 	= tokenizer.nextToken();
			String nxtAction	= tokenizer.nextToken();
			
			if (nxtName.equals("marvin")) {
				
				LCD.clear(6);
				LCD.clear(7);
				
				switch (nxtCommand.charAt(0)) {
					
					case 'v':
						
						if (nxtAction.startsWith("-")) {						
							engine.driveBackward();
							engine.changeSpeed(Integer.parseInt(nxtAction.substring(1)));
						} else {								
							if (!usSensor.driveToHell()) {
								engine.driveForward();
								engine.changeSpeed(Integer.parseInt(nxtAction.substring(1)));
							} else {
								LCD.drawString("Drive to Hell...", 0, 6);
							}						
						}
						
						break;
					case 'w':
						
												
						if (nxtAction.startsWith("-")) {
							wheel.turnLeft(Integer.parseInt(nxtAction.substring(1)));
						} else {
							wheel.turnRight(Integer.parseInt(nxtAction));
						}
						
						break;
					case 'c':
						
						if (nxtAction.equals("turnL")) {
							camera.turnLeft();
						} else if (nxtAction.equals("turnR")) {
							camera.turnRight();
						} else if (nxtAction.equals("stop")) {
							camera.stopCamera();
						}
						
						break;
					case 's':
						engine.stopDriving();
						wheel.turnLeft(0);						
						break;
					default:
						LCD.drawString("cmd not unds.\n" + command, 0, 6);
						break;
				
				}
				
			} else {
				LCD.drawString("not marvin\n" + command, 0, 6);
			}
			
		} else {
			LCD.drawString("cmd incorrect\n" + command, 0, 6);
		}	
		
	}
	
	
	
}
